import numpy as np
import matplotlib.pyplot as plt
import pandas as pd
import math
# 地球半径，单位为米
R = 6378137.0


# 将度数转换为弧度
def deg2rad(deg):
    return deg * (math.pi / 180)


# 批量转换经纬度数据为x, y直角坐标
def PVT2ENU(locations, base_loc):
    # 将纬度和经度差转换为弧度
    delta_lat = deg2rad(locations[:, 0] - base_loc[0])  # 纬度差
    delta_lon = deg2rad(locations[:, 1] - base_loc[1])  # 经度差

    # 基准点的纬度转换为弧度
    lat_base_rad = deg2rad(base_loc[0])

    # 计算Y（北向坐标）
    y = R * delta_lat

    # 计算X（东向坐标），需要乘以纬度的余弦
    x = R * delta_lon * np.cos(lat_base_rad)

    # 返回 (x, y) 坐标
    return np.stack((x, y), axis=1)

def sensor_interp(tmsp, sdata):
    sync_cols = []
    for col_idx in range(sdata.shape[1]):
       sync_sdata = np.interp(tmsp, sdata[:, 0], sdata[:, col_idx])
       sync_cols.append(sync_sdata)
    sync_sdata = np.stack(sync_cols, axis=1)
    return sync_sdata

loca_path = '../dataset/No_81_Denggao_Road-2024-10-03_05-19-03/Location.csv'
acce_path = '../dataset/No_81_Denggao_Road-2024-10-03_05-19-03/Accelerometer.csv'
location_data = pd.read_csv(loca_path)
time_data = pd.read_csv(acce_path)
index = int(len(location_data) * 0.01)
location_data = location_data[index:len(location_data)-index]
print(location_data.shape)
#print(len(location_data))
time_data = time_data['time']
#loacation_data = sensor_interp(time_data, loacation_data)
#time, latitude, longitude = loacation_data['time'], loacation_data['latitude'], loacation_data['longitude']
gps_data = []
base_loc = []
time_stmp = []
for ti, la, lo in zip(location_data['time'], location_data['latitude'], location_data['longitude']):
    gps_data.append([ti, la, lo])

for i in range(len(time_data)):
    time_stmp.append(time_data[i])
gps_data = np.array(gps_data)
time_stmp = np.array(time_stmp)
base_loc = gps_data[0][1:]
'''
# interpolation
interp_data = sensor_interp(time_stmp, gps_data)
interp_data = interp_data[:, 1:]
xy_point = PVT2ENU(interp_data, base_loc)

xy_point = PVT2ENU(gps_data[:, 1:], base_loc)
print(xy_point.shape)
# 计算路径

# 绘制路径

x, y = xy_point[:, 0], xy_point[:, 1]
#x, y = x[:2], y[:2]

print(base_loc,'---------base')
print(gps_data[:, 1:][0])
print(gps_data[:, 1:][1])
for i, j in zip(x, y):
    print(i, j,'===========')

plt.plot(x, y)

plt.scatter(x[0], y[0], color='green', s=100, label='Start')  # 起点标注为绿色
plt.scatter(x[-1], y[-1], color='red', s=100, label='End')  # 终点标注为红色

# 添加注释文本到起点和终点
plt.annotate('Start', (x[0], y[0]), textcoords="offset points", xytext=(10,-10), ha='center')
plt.annotate('End', (x[-1], y[-1]), textcoords="offset points", xytext=(10,-10), ha='center')
plt.title('ENU Path')

plt.xlabel('East (meters)')
plt.ylabel('North (meters)')
plt.grid()
plt.axis('equal')
plt.show()

'''
plt.plot(x_coords, y_coords, marker='o')
plt.title("GPS Path")
plt.xlabel("X Coordinate (meters)")
plt.ylabel("Y Coordinate (meters)")
plt.grid()
plt.show()'''